Probabilistic Roadmap Method

This paper presents Probabilistic Roadmap Method (PRM) simulation with straightline local planner [1]. I set up a simulation environment with 7-links robot, obstacles (bombs), and a goal. First of all, I generated samples in free configuration space. Second, if one sample could be connected to its neighbor samples, store these two samples in a graph with a cost (L-metric distance between two samples). Finally, the path, from start point to the goal, could be found by simple graph search algorithm. I. 7-Links Robot Implementation Figure 1 shows that my 7-links robot and its DH-parameters. The robot has 2spherical joints, and I added up two Ghost-axis (z2 and z5) arbitrarily; because this is the only way, if we want to start 7-links robot simulation from right stand-up position under DH convention. To clarify it, let’s remind the two constraints for DH-representation [2]: (DH1) The axis x1 is perpendicular to the axis z0. (DH2) The axis x1 intersects the axis z0. θi*: variables Link Actual Link ai αi di θi 1 2 3 4 5 6 7 8 9 1 2 Ghost 3 4 Ghost 5 6 7 0 0 0 0 0 0 0 1.0 0.5 -90 90 0 -90 90 0 -90 0 0 0 0 1.5+1.0 0 0 1.5+1.0 0 0 0 θ1* θ2* fixed θ4* θ5* fixed θ7* θ8* θ9* Figure 1. 7-links robot and DH-parameters. Also, I was thinking of the extension of this idea to serpentine type robot simulation (as you know, every joint of snake type robot can be represented as a spherical joint), this technique will be helpful unless you would like to move your snake robot from any crooked posture at the beginning. We can determine the position of link3 and link5 by easy (probably middle school level) vector calculation (see Fig 2.). n OA m OB OP m n × + × = + Figure 2. Vector calculation to determine the position of link 3 and link 5. II. Sample Connection First, I declared a data type typedef struct element { int d1, d2, d4, d5, d7, d8, d9; bool collision; }; struct element sample[MAX_SAMPLE]; struct element free_sample[MAX_SAMPLE]; If a randomly generated sample is in collision free space, then store it to free_sample array. These free_sample become vertices vi in graph G. Second, I used the adjacency matrix from to represent graph structure [3]; thus if G=(V, E) is a graph with n vertices, n≥1, the adjacency matrix of G is a twodimensional n×n array, say cost. If there exists the edge (vi, vj) that can connect two nodes with straight-line planner in E(G), then cost[i][j] = L_distance between two nodes (samples). If there is no such edge in E(G), cost[i][j] = ∞ (also if L-distance > neighborhood). Figure 3 shows an illustrative example. Finally, Dijkstra’s algorithm (function shortest_path below) was used to find the shortest paths. void shortest_path(int v, double cost[MAX_VERTICES][MAX_VERTICES], int path[], int n,